/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0

Bump_TerrainPatch.cpp

This file implements the terrain patch class.

*/
#include "Bump_TerrainPatch.h"


//Orulex types
typedef union{
 double data[4];
 struct {double x,y,z,m;};
}VECTOR4;

extern double ORUGetElevByVec(VECTOR4 pos);
extern VECTOR4 v32v4(VECTOR3 v);

//Debugging
extern char boutput[1000];



/**
 * Constructor for class, doesn't create terrain
 */
Bump_TerrainPatch::Bump_TerrainPatch(unsigned int lngMaxPoints, unsigned int latMaxPoints,
									 double lngAngleIncr, double latAngleIncr,
									 double lngBorderWidth, double latBorderWidth,
									 double updateDistance)
{
	this->lngMaxPoints = lngMaxPoints;
	this->latMaxPoints = latMaxPoints;
	lngMaxQuads  = lngMaxPoints - 1;
	latMaxQuads  = latMaxPoints - 1;
	maxIndices = (lngMaxQuads)*(latMaxQuads) * 2 * 3;  //No. of triangles

	vertices = new btVector3[lngMaxPoints * latMaxPoints];
	indices  = new int[maxIndices];

	this->lngAngleIncr = lngAngleIncr; //0.000002f;
	this->latAngleIncr = latAngleIncr; //0.000002f;

	this->lngBorderWidth    = lngBorderWidth;
	this->latBorderWidth    = latBorderWidth;
	this->updateDistance = updateDistance;

	lngMidpoint = ((double)lngMaxQuads/2.0f);
	latMidpoint = ((double)latMaxQuads/2.0f);

	rbTerrainPatch = NULL;
	collshpTerrainPatch = NULL;
	m_indexVertexArrays = NULL;

}


/**
 * Destructor, frees memory before destruction
 */
Bump_TerrainPatch::~Bump_TerrainPatch()
{


	if(m_indexVertexArrays){

		//delete triangle vertex array, this is required
		delete m_indexVertexArrays;

		m_indexVertexArrays = NULL;

		sprintf(boutput, "~Bump_TerrainPatch: Terrain Patch deleted %p, %p, %p",
								rbTerrainPatch, collshpTerrainPatch, m_indexVertexArrays);
		oapiWriteLog(boutput);
	}

	if(vertices)
		delete[] vertices;

	if(indices)
		delete[] indices;

}



/**
 * Update the patch if needed.
 *
 * TODO: Shift all pre-condition out if possible, has problems when inlining
 */
int
Bump_TerrainPatch::updatePatch(Bump_BulletBase *base, double lng, double lat, double rad)
{
	//Add/remove vertices based on the direction of supported body movement

/*	sprintf(boutput, "Bump_TerrainPatch::updatePatch: CALLED lng=%f, lat=%f, base->lng=%f, base->lat=%f",
				lng, lat, base->lng,  base->lat);
	oapiWriteLog(boutput);*/

	bool recenterTerrain = false;
	int rv;

	if(rbTerrainPatch){

		//Check if terrain is close enough to body to justify an update

		//Global direction and position vectors
		VECTOR3 gdirSurfPointRadiusVector;
		VECTOR3 gposMoon, gposSurfPoint;
		double currentDistFromTerrain;

		//Must use base radius here as we are getting the gpos for a surface point(for Orulex)
		oapiEquToGlobal(base->hObjRefBody, lng, lat, base->radRefBody, &gposSurfPoint);

		//Vector from Moon center to surface point in global frame
		oapiGetGlobalPos(base->hObjRefBody, &gposMoon);
		gdirSurfPointRadiusVector = gposSurfPoint - gposMoon;

		//Elevation measured from the surface
		currentDistFromTerrain = rad - ORUGetElevByVec(v32v4(gdirSurfPointRadiusVector));

		if(currentDistFromTerrain > updateDistance){
			/*sprintf(boutput, "updatePatch: currentDistFromTerrain = %f is > updateDistance = %f, skipping",
					currentDistFromTerrain, updateDistance);
			oapiWriteLog(boutput);*/
			return 1;
		}




		if(lng < (equLngStart + lngBorderWidth)){
			/*sprintf(boutput, "updatePatch: lng < (equLngStart + borderWidth) : %f < (%f + %f)",
					lng, equLngStart, borderWidth);*/
			recenterTerrain = true;
		}
		else if(lng > (equLngEnd - lngBorderWidth)){
			/*sprintf(boutput, "updatePatch: lng > (equLngEnd - borderWidth) : %f > (%f - %f)",
								lng, equLngEnd, borderWidth);*/
			recenterTerrain = true;
		}
		else if(lat < (equLatStart + latBorderWidth)){
			/*sprintf(boutput, "updatePatch: lat < (equLatStart + borderWidth) : %f < (%f + %f)",
								lat, equLatStart, borderWidth);*/
			recenterTerrain = true;
		}
		else if(lat > (equLatEnd - latBorderWidth)){
			/*sprintf(boutput, "updatePatch: lat > (equLatEnd - borderWidth) : %f > (%f - %f)",
								lat, equLatEnd, borderWidth);*/
			recenterTerrain = true;
		}
		else
			return 2;  //not touching borders, do nothing


		if(recenterTerrain){
			/*oapiWriteLog(boutput);*/

			//Set up the limits of this patch
			equLngStart = lng - lngMidpoint*((double)lngAngleIncr);
			equLatStart = lat - latMidpoint*((double)latAngleIncr);

			//used to chk later, that the terrain was generated till correct point on the surface
			//(can't have overlapping triangles)
			//Increments occur MAX_QUADS = MAX_POINTS-1 times as starting lola is 1 value already
			equLngEnd = equLngStart + lngMaxQuads*lngAngleIncr;
			equLatEnd = equLatStart + latMaxQuads*latAngleIncr;

			/*sprintf(boutput, "Bump_TerrainPatch::updatePatch: Start(%f, %f) : End(%f, %f)", equLngStart, equLatStart,
					equLngEnd, equLatEnd);
			oapiWriteLog(boutput);*/

			//Set vertices and indices according to Orulex output
			rv = setTerrainVerticesORU(base);
			if(rv){
				sprintf(boutput, "Bump_TerrainPatch::updatePatch: setTerrainVertices() failed with error : %d !!", rv);
				oapiWriteLog(boutput);
				return 3;
			}


			collshpTerrainPatch->refitTree(aabbMin,aabbMax);

			//clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
			base->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(rbTerrainPatch->getBroadphaseHandle(),base->m_dynamicsWorld->getDispatcher());
		}

		return 0;  //update occurred correctly, this is the best case for this function
	}


	return 4;
}


/**
* Creates triangle mesh for terrain patch, does not add to bullet world
* Is false biased, only if nothing goes wrong will it return true.
*
* TODO: Surface reference of camera and this base must match, else the object on the terrain
* must be frozen. Orulex wont generate terrain for a planet not closest to the camera.
*/
int
Bump_TerrainPatch::createPatch(Bump_BulletBase *base, double lng, double lat)
{

	int rv, vertStride, indexStride;

	sprintf(boutput, "createPatch: CALLED lng=%f, lat=%f, base->lng=%f, base->lat=%f, lngMidPoint=%f",
			lng, lat, base->lng,  base->lat, lngMidpoint);
	oapiWriteLog(boutput);


	//Set up the limits of this patch

	equLngStart = lng - lngMidpoint*((double)lngAngleIncr);
	equLatStart = lat - latMidpoint*((double)latAngleIncr);

	//used to chk later, that the terrain was generated till correct point on the surface
	//(can't have overlapping triangles)
	//Increments occur MAX_QUADS = MAX_POINTS-1 times as starting lola is 1 value already
	equLngEnd = equLngStart + lngMaxQuads*lngAngleIncr;
	equLatEnd = equLatStart + latMaxQuads*latAngleIncr;

	sprintf(boutput, "createPatch: Start(%f, %f) : End(%f, %f)", equLngStart, equLatStart,
			equLngEnd, equLatEnd);
	oapiWriteLog(boutput);

/*	//Calculate base relative patch start point(lower corner)
	VECTOR3 gposSurfPoint;

	//Convert lola to global point on surface of this base's reference body
	oapiEquToGlobal(base->hObjRefBody, lng, lat, base->rad, &gposSurfPoint);

	//No need to add gposBB(bullet base global position) here,
	//as we are converting the surface point position to base frame
	base->bb->Global2Local(gposSurfPoint, rposSurfStartPoint);

	sprintf(boutput, "createPatch: Start Point (%f, %f, %f)", V3ARGS(rposSurfStartPoint));
	oapiWriteLog(boutput);
*/

	//Set vertices and indices according to Orulex output
	rv = setTerrainVerticesORU(base);
	if(rv){
		sprintf(boutput, "createPatch: setTerrainVertices() failed with error : %d !!", rv);
		oapiWriteLog(boutput);
		return 1;
	}

	//Finally make the triangle mesh from the vertices and indices array, this is just done ONCE
	vertStride = sizeof(btVector3);
	indexStride = 3*sizeof(int);
	m_indexVertexArrays = new btTriangleIndexVertexArray(   //THIS NEEDS TO BE FREED
			(lngMaxQuads*latMaxQuads*2),
			indices,
			indexStride,
			maxIndices,
			(btScalar*) &(vertices[0].x()),
			vertStride);


	bool useQuantizedAabbCompression = true;
	collshpTerrainPatch = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,
													 aabbMin,aabbMax);



	//We have the collision shape, now make the rigid body & insert into dynamics world
	if(collshpTerrainPatch){

		btTransform transStartPos(btQuaternion(btVector3(0,1,0), 0.f), btVector3(0,0,0));

		base->m_collisionShapes->push_back(collshpTerrainPatch);

		//Motion state not used as this object is static and will never be synchronized after
		//creation. It will only be deleted after this.
		rbTerrainPatch = base->localCreateRigidBody(0.f, transStartPos, collshpTerrainPatch);
		if(!rbTerrainPatch)
			return 2; //error, rigid body for triangle mesh not created !!

		//May need kinematic flag
		rbTerrainPatch->setCollisionFlags(rbTerrainPatch->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);//CF_KINEMATIC_OBJECT);//);

	}

	return 0;
}


/**
 * Creates triangle mesh from Orulex data
 */
inline int
Bump_TerrainPatch::setTerrainVerticesORU(Bump_BulletBase *base)
{
	unsigned int i, j, k;
	int markerStep = 0;
	double lng, lat, elev;

	//Global direction and position vectors
	VECTOR3 gdirSurfPointRadiusVector;
	VECTOR3 gposMoon, gposSurfPoint, gposBB, gposElevatedPoint;

	//Relative to vessel(base)
	VECTOR3 rposElevatedPoint;

	aabbMin.setValue( BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
	aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);


	/*sprintf(boutput, "setTerrainVertices: CALLED lng=%f, lat=%f,
	  	  	  lngAngleIncr=%f, latAngleIncr=%f", equLngStart, equLatStart, lngAngleIncr, latAngleIncr);
	oapiWriteLog(boutput);*/

	//First generate the points matrix in base frame


	//Get reference body(nearest celestial body) co-ordinates, this keeps changing
	oapiGetGlobalPos(base->hObjRefBody, &gposMoon);
	base->bb->GetGlobalPos(gposBB);



	//lng = -0.582085;
	lng = equLngStart;
	k = -1;
	for(i=0; i<lngMaxPoints; ++i){

		//lat = 0.717760;
		lat = equLatStart;
		for(j=0; j<latMaxPoints; ++j){

			//Must use base radius here as we are getting the gpos for a surface point(for Orulex)
			oapiEquToGlobal(base->hObjRefBody, lng, lat, base->radRefBody, &gposSurfPoint);

			//Vector from Moon center to surface point in global frame
			gdirSurfPointRadiusVector = gposSurfPoint - gposMoon;

			//Elevation measured from the surface
			elev = ORUGetElevByVec(v32v4(gdirSurfPointRadiusVector));

			gposElevatedPoint = unit(gdirSurfPointRadiusVector) * elev;  //elevation calculations in global frame, removes 1 function call & 1 mul.

			gposElevatedPoint += gposMoon; //translate above the moon from global origin


			base->bb->Global2Local(gposElevatedPoint, rposElevatedPoint);

		/*	//No need to add gposBB here, as we are converting the surface point position to base frame
			base->bb->Global2Local(gposSurfPoint, rposSurfPoint);

			//Get the unit vector in direction of radius vector of the surface point, in base frame
			rdirUnitSurfPointRadiusVector = unit(rdirSurfPointRadiusVector);

			//Add the scaled unit vector to the surface point position in base frame
			//to get elevated point in base frame
			rposElevatedPoint = rposSurfPoint + rdirUnitSurfPointRadiusVector * elev;*/

			//Mesh addition logic
		/*	if( (i%20==0 && j==0) || (i%(MAX_POINTS-1)==0 && j==0) ||
				(i%20==0 && j==(MAX_POINTS-1)) || (i%(MAX_POINTS-1)==0 && j==(MAX_POINTS-1))	)
				base->bb->AddMesh("Bump/DebugMeshes/ArrowMarker", &rposElevatedPoint);*/





			vertices[i*lngMaxPoints + j].setValue(btScalar(rposElevatedPoint.x),
												btScalar(rposElevatedPoint.y),
												btScalar(rposElevatedPoint.z));

			aabbMax.setMax(vertices[i*lngMaxPoints + j]);
			aabbMin.setMin(vertices[i*lngMaxPoints + j]);

		/*	sprintf(boutput, "Bump: lng = %f(%f), lat = %f(%f) : elev : %f, ANGLE_INCREMENT : %f, rposElevatedPoint = %f,%f,%f, \
					aabbMax=(%f,%f,%f), aabbMin=(%f,%f,%f)",
					lng, (lng*180/PI), lat, (lat*180/PI), elev, ANGLE_INCREMENT, V3ARGS(rposElevatedPoint),
					aabbMax.x(), aabbMax.y(), aabbMax.z(), aabbMin.x(), aabbMin.y(), aabbMin.z());


			oapiWriteLog(boutput);*/

			//Generate indices for 1 quad at a time, 2 triangles per quad.
			//i & j should go upto MAX_POINTS - 2 = MAX_QUADS - 1 => i,j < MAX_QUADS
			if(i<lngMaxQuads && j<latMaxQuads){
				//1st triangle of this quad
				indices[++k] = i*lngMaxPoints + j;			//0
				indices[++k] = i*lngMaxPoints + (j+1);		//1
				indices[++k] = (i+1)*lngMaxPoints + j;		//3

				indices[++k] = (i+1)*lngMaxPoints + j;		//3
				indices[++k] = i*lngMaxPoints + (j+1);		//1
				indices[++k] = (i+1)*lngMaxPoints + (j+1);	//4
			}


			lat += latAngleIncr;

		}//for-j

		lng += lngAngleIncr;
	}//for-i



	//Points recorded, print to chk
/*	for(i=0; i<MAX_POINTS; i++){
		for(j=0; j<MAX_POINTS; j++){

			//aabbMax.setMax(vertices[i*MAX_POINTS + j]);
			//aabbMin.setMin(vertices[i*MAX_POINTS + j]);
			//aabbMax.setValue(10000,10000,10000);
			//aabbMin.setValue(-10000,-10000,-10000);

			sprintf(boutput, "Bump: vertices[%d] = %f, %f, %f, aabbMin(%f,%f,%f), aabbMax(%f,%f,%f)",
				i*MAX_POINTS +j,
				vertices[i*MAX_POINTS +j].x(),
				vertices[i*MAX_POINTS +j].y(),
				vertices[i*MAX_POINTS +j].z(),
				aabbMin.x(), aabbMin.y(), aabbMin.z(),
				aabbMax.x(), aabbMax.y(), aabbMax.z());
			oapiWriteLog(boutput);
		}
	}
*/

	//Print indices
/*	for( k=0; k<(MAX_QUADS*MAX_QUADS*3*2); k+=(3*2) ){

		sprintf(boutput, "Bump: -----Quad : %d-----", k/6);
		oapiWriteLog(boutput);

		sprintf(boutput, "Bump: Triangle : %d : indices[%d] = %d, indices[%d] = %d, indices[%d] = %d",
			k/3,
			k,   indices[k],
			k+1, indices[k+1],
			k+2, indices[k+2] );
		oapiWriteLog(boutput);

		sprintf(boutput, "Bump: Triangle : %d : indices[%d] = %d, indices[%d] = %d, indices[%d] = %d",
			(k+3)/3,
			(k+3),   indices[(k+3)],
			(k+3)+1, indices[(k+3)+1],
			(k+3)+2, indices[(k+3)+2] );
		oapiWriteLog(boutput);

	}
*/
	//Print covered lola range
	lng -= lngAngleIncr;
	lat -= latAngleIncr;

	/*sprintf(boutput, "Bump: COVERED : lola(%f,%f) TO lola(%f,%f)", equLngStart, equLatStart, lng, lat);
	oapiWriteLog(boutput);*/

	//Check if lola ended as expected
	//TODO: Comment out this check later
	if(lat != equLatEnd || lng != equLngEnd){
		sprintf(boutput, "Bump: COVERED : ERROR Expected lola to cover till lo:%f, la:%f", equLngEnd, equLatEnd);
		oapiWriteLog(boutput);
	}




	return 0;
}

